/*
 * @Description: 订阅velocity数据
 * @Author: Zhijian Qiao
 * @Date: 2019-08-19 19:22:17
 */
#ifndef avp_slam_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_
#define avp_slam_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include "nav_msgs/Odometry.h"

#include <gazebo_msgs/LinkStates.h>
#include <geometry_msgs/Twist.h>
#include "sensor_data/velocity_data.hpp"
#include <geometry_msgs/TwistWithCovarianceStamped.h>

namespace avp_slam {
    class VelocitySubscriber {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VelocitySubscriber(ros::NodeHandle &nh, std::string topic_name, size_t buff_size,
                           const Eigen::Matrix4f &car_to_imu);

        VelocitySubscriber() = default;

        void ParseData(std::deque<VelocityData> &deque_velocity_data);

    private:
        void msg_callback(const gazebo_msgs::LinkStatesConstPtr &links_ptr);

    public:
        ros::NodeHandle nh_;
        ros::Subscriber subscriber_;
        std::deque<VelocityData> new_velocity_data_;
        Eigen::Matrix4f car_to_imu_;
        std::mutex buff_mutex_;
        double last_time_ = 0.0;
        VelocityData nonZeroLast;
    };
}
#endif